#include "../Matrix.h"
#include "../VectorOperations.h"
#include "Fustrum.h"

Fustrum::Fustrum()
{
	myPosition.Set(0.0f, 0.0f, 0.0f);
	for (unsigned int index=0; index<myPoints.size(); ++index)
	{
		myPoints[index] = myPosition;
	}
	myFOV = 0;
	myRatio = 0;
	myNearDistance = 0;
	myFarDistance = 0;
	myNearPoint = myFarPoint = myPosition;
	myToWorldVector = myPosition;
}

Fustrum::~Fustrum()
{
}

void Fustrum::Init(const FustrumSetup &aSetup)
{
	myPosition.Set(0.0f, 0.0f, 0.0f);
	myFOV = aSetup.fov;
	myRatio = static_cast<float>(aSetup.res.x) / static_cast<float>(aSetup.res.y);
	myNearDistance = aSetup.nearPlane;
	myFarDistance = aSetup.farPlane;
	float nearHeight = 2.0f * tan((myFOV/myRatio) * 0.5f) * myNearDistance;
	float nearWidth = nearHeight * myRatio;
	float farHeight = 2.0f * tan((myFOV/myRatio) * 0.5f) * myFarDistance;
	float farWidth = farHeight * myRatio;
	Vector3f up(0.0f, 1.0f, 0.0f);
	Vector3f right(1.0f, 0.0f, 0.0f);
	Vector3f normal = -up.Cross(right);
	myFarPoint = myPosition + normal * myFarDistance;
	myNearPoint = myPosition + normal * myNearDistance;

	Vector3f &ftl = myPoints[F_FAR_TOP_LEFT];
	Vector3f &ftr = myPoints[F_FAR_TOP_RIGHT];
	Vector3f &fbl = myPoints[F_FAR_BOTTOM_LEFT];
	Vector3f &fbr = myPoints[F_FAR_BOTTOM_RIGHT];

	Vector3f &ntl = myPoints[F_NEAR_TOP_LEFT];
	Vector3f &ntr = myPoints[F_NEAR_TOP_RIGHT];
	Vector3f &nbl = myPoints[F_NEAR_BOTTOM_LEFT];
	Vector3f &nbr = myPoints[F_NEAR_BOTTOM_RIGHT];

	ftl = myFarPoint + (up * farHeight*0.5f) - (right * farWidth*0.5f);
	ftr = myFarPoint + (up * farHeight*0.5f) + (right * farWidth*0.5f);
	fbl = myFarPoint - (up * farHeight*0.5f) - (right * farWidth*0.5f);
	fbr = myFarPoint - (up * farHeight*0.5f) + (right * farWidth*0.5f);

	ntl = myNearPoint + (up * nearHeight*0.5f) - (right * nearWidth*0.5f);
	ntr = myNearPoint + (up * nearHeight*0.5f) + (right * nearWidth*0.5f);
	nbl = myNearPoint - (up * nearHeight*0.5f) - (right * nearWidth*0.5f);
	nbr = myNearPoint - (up * nearHeight*0.5f) + (right * nearWidth*0.5f);

	myPlanes[F_NEAR].Init(myNearPoint, normal);
	myPlanes[F_FAR].Init(myFarPoint, -normal);

	Vector3f point = (ntr + nbr + ftr + fbr) * 0.25f;
	Vector3f a = point - myPosition;
	a.Normalize();
	//RIGHT
	normal = -up.Cross(a);
	myPlanes[F_RIGHT].Init(point, normal);

	point = (ntl + nbl + ftl + fbl) * 0.25f;
	a = point - myPosition;
	a.Normalize();
	//LEFT
	normal = -up.Cross(-a);
	myPlanes[F_LEFT].Init(point, normal);

	point = (ntl + ntr + ftl + ftr) * 0.25f;
	a = point - myPosition;
	a.Normalize();
	//TOP
	normal = -right.Cross(-a);
	myPlanes[F_TOP].Init(point, normal);

	point = (nbl + nbr + fbl + fbr) * 0.25f;
	a = point - myPosition;
	a.Normalize();
	//BOTTOM
	normal = -right.Cross(a);
	myPlanes[F_BOTTOM].Init(point, normal);

	myToWorld = Matrix33f::CreateRotate(aSetup.rot);
	myToWorldVector = aSetup.pos;

	CacheFrustrum();
}

FrustumIntersection Fustrum::PointVsFrustum(const Vector3f &aPoint) const
{
	float distance = 0;
	FrustumIntersection result = FRUSTUM_IN;
	for (unsigned int index=0; index<myPlanes.size(); ++index)
	{
		//Vector3f normal = (myPlanes[index].myNormal * myToWorld).Normalize();
		//Vector3f point = myPlanes[index].myPoint * myToWorld + myToWorldVector;
		//distance = normal.Dot(aPoint-point);
		distance = myCachedNormals[index].Dot(aPoint) + myCachedDistances[index];
		if (distance<0)
		{
			return FRUSTUM_OUT;
		}
		if (distance==0)
		{
			result = FRUSTUM_INTERSECT;
		}
	}
	return result;
}

FrustumIntersection Fustrum::SphereVsFrustum(const Intersection::Sphere &aSphere) const
{
	float distance = 0;
	FrustumIntersection result = FRUSTUM_IN;
	for (unsigned int index=0; index<myPlanes.size(); ++index)
	{
		//Vector3f normal = (myPlanes[index].myNormal * myToWorld).Normalize();
		//Vector3f point = myPlanes[index].myPoint * myToWorld + myToWorldVector;
		//distance = normal.Dot(aSphere.myCenterPosition-point);
// 		float planeDistance = 0;
// 		if(index == F_NEAR)
// 		{
// 			planeDistance = myNearDistance;
// 		}
// 		else if(index == F_FAR)
// 		{
// 			planeDistance = myFarDistance;
// 		}

		distance = myCachedNormals[index].Dot(aSphere.myCenterPosition) + myCachedDistances[index];
		if (distance<-aSphere.myRadius)
		{
			return FRUSTUM_OUT;
		} 
		else if (distance<aSphere.myRadius)
		{
			result = FRUSTUM_INTERSECT;
		}
	}
	return result;
}

FrustumIntersection Fustrum::AabbVsFrustum(const Intersection::AABB &anAabb) const
{
	Vector3f positive(anAabb.myMinPos);
	Vector3f negative(anAabb.myMaxPos);
	float distance = 0;
	FrustumIntersection result = FRUSTUM_IN;
	for (unsigned int index=0; index<myPlanes.size(); ++index)
	{
		//Vector3f normal = (myPlanes[index].myNormal * myToWorld).Normalize();
		Vector3f normal = myCachedNormals[index];
		//Vector3f point = myPlanes[index].myPoint * myToWorld + myToWorldVector;
		//Vector3f point = myCachedPoints[index];
		positive = anAabb.myMinPos;
		if (normal.x>=0)
		{
			positive.x = anAabb.myMaxPos.x;
		}
		if (normal.y>=0)
		{
			positive.y = anAabb.myMaxPos.y;
		}
		if (normal.z>=0)
		{
			positive.z = anAabb.myMaxPos.z;
		}
		negative = anAabb.myMaxPos;
		if (normal.x>=0)
		{
			negative.x = anAabb.myMinPos.x;
		}
		if (normal.y>=0)
		{
			negative.y = anAabb.myMinPos.y;
		}
		if (normal.z>=0)
		{
			negative.z = anAabb.myMinPos.z;
		}
		distance = normal.Dot(positive) + myCachedDistances[index];
		if (distance<0)
		{
			return FRUSTUM_OUT;
		}
		distance = normal.Dot(negative) + myCachedDistances[index];
		if (distance<0)
		{
			result = FRUSTUM_INTERSECT;
		}
	}
	return result;
}

void Fustrum::GetTransformation(Matrix44f &aMatrix)
{
	for (unsigned short i=0; i<3; i++)
	{
		for (unsigned short j=0; j<3; j++)
		{
			aMatrix[i + 4*j] = myToWorld[i + 3*j];
		}
	}
	aMatrix[12] = myToWorldVector.x;
	aMatrix[13] = myToWorldVector.y;
	aMatrix[14] = myToWorldVector.z;
}

void Fustrum::RecalculateFromPoints()
{
	float nearHeight = 2.0f * tan(myFOV * 0.5f) * myNearDistance;
	float nearWidth = nearHeight * myRatio;
	float farHeight = 2.0f * tan(myFOV * 0.5f) * myFarDistance;
	float farWidth = farHeight * myRatio;
	Vector3f up(0.0f, 1.0f, 0.0f);
	Vector3f right(1.0f, 0.0f, 0.0f);
	Vector3f normal = -up.Cross(right);

	Vector3f &ftl = myPoints[F_FAR_TOP_LEFT];
	Vector3f &ftr = myPoints[F_FAR_TOP_RIGHT];
	Vector3f &fbl = myPoints[F_FAR_BOTTOM_LEFT];
	Vector3f &fbr = myPoints[F_FAR_BOTTOM_RIGHT];

	Vector3f &ntl = myPoints[F_NEAR_TOP_LEFT];
	Vector3f &ntr = myPoints[F_NEAR_TOP_RIGHT];
	Vector3f &nbl = myPoints[F_NEAR_BOTTOM_LEFT];
	Vector3f &nbr = myPoints[F_NEAR_BOTTOM_RIGHT];

	ftl = myFarPoint + (up * farHeight*0.5f) - (right * farWidth*0.5f);
	ftr = myFarPoint + (up * farHeight*0.5f) + (right * farWidth*0.5f);
	fbl = myFarPoint - (up * farHeight*0.5f) - (right * farWidth*0.5f);
	fbr = myFarPoint - (up * farHeight*0.5f) + (right * farWidth*0.5f);

	ntl = myNearPoint + (up * nearHeight*0.5f) - (right * nearWidth*0.5f);
	ntr = myNearPoint + (up * nearHeight*0.5f) + (right * nearWidth*0.5f);
	nbl = myNearPoint - (up * nearHeight*0.5f) - (right * nearWidth*0.5f);
	nbr = myNearPoint - (up * nearHeight*0.5f) + (right * nearWidth*0.5f);

	myPlanes[F_NEAR].Init(myNearPoint, normal);
	myPlanes[F_FAR].Init(myFarPoint, -normal);

	Vector3f point = (ntr + nbr + ftr + fbr) * 0.25f;
	Vector3f a = point - myPosition;
	a.Normalize();
	//RIGHT
	normal = -up.Cross(a);
	myPlanes[F_RIGHT].Init(point, normal);

	point = (ntl + nbl + ftl + fbl) * 0.25f;
	a = point - myPosition;
	a.Normalize();
	//LEFT
	normal = -up.Cross(-a);
	myPlanes[F_LEFT].Init(point, normal);

	point = (ntl + ntr + ftl + ftr) * 0.25f;
	a = point - myPosition;
	a.Normalize();
	//TOP
	normal = -right.Cross(-a);
	myPlanes[F_TOP].Init(point, normal);

	point = (nbl + nbr + fbl + fbr) * 0.25f;
	a = point - myPosition;
	a.Normalize();
	//BOTTOM
	normal = -right.Cross(a);
	myPlanes[F_BOTTOM].Init(point, normal);

	CacheFrustrum();
}

void Fustrum::SetTransformation(const Matrix44f &aMatrix)
{
	for (unsigned short i=0; i<3; i++)
	{
		for (unsigned short j=0; j<3; j++)
		{
			myToWorld[i + 3*j] = aMatrix[i + 4*j];
		}
	}
	myToWorldVector.x = aMatrix[12];
	myToWorldVector.y = aMatrix[13];
	myToWorldVector.z = aMatrix[14];

	CacheFrustrum();
}

Vector3f Fustrum::ToFrustumSpace(const Vector3f &aVector)
{
	Vector3f point = (aVector - myToWorldVector) * ~myToWorld;
	return point;
}

Vector3f Fustrum::ToWorldSpace(const Vector3f &aVector)
{
	Vector3f point = aVector * myToWorld + myToWorldVector;
	return point;
}

void Fustrum::CacheFrustrum()
{
// 	for (unsigned int index=0; index<myPlanes.size(); ++index)
// 	{
// 		myCachedNormals[index] = (myPlanes[index].myNormal * myToWorld).Normalize();
// 		//myCachedPoints[index] = myPlanes[index].myPoint * myToWorld + myToWorldVector;
// 
// 		if(index == F_NEAR)
// 		{
// 			myCachedDistances[index] = (myToWorldVector + (Vector3f(0,0,myNearDistance) * myToWorld)).Length();
// 		}
// 		else if(index == F_FAR)
// 		{
// 			myCachedDistances[index] = (myToWorldVector + (Vector3f(0,0,myFarDistance) * myToWorld)).Length();
// 		}
// 		else
// 		{
// 			myCachedDistances[index] = myToWorldVector.Length();
// 		}
// 		
// 	}
	for (unsigned int index=0; index<myPoints.size(); ++index)
	{
		myCachedBoxPoints[index] = myPoints[index] * myToWorld + myToWorldVector;
	}
}

void Fustrum::Test( Matrix44f& aOrientation, Matrix44f& aProjection, Matrix44f& aRealOrientation )
{
	Matrix44f clip;// = aProjection * aOrientation;
	clip[0] = aOrientation[ 0] * aProjection[ 0] + aOrientation[ 1] * aProjection[ 4] + aOrientation[ 2] * aProjection[ 8] + aOrientation[ 3] * aProjection[12];
	clip[1] = aOrientation[ 0] * aProjection[ 1] + aOrientation[ 1] * aProjection[ 5] + aOrientation[ 2] * aProjection[ 9] + aOrientation[ 3] * aProjection[13];
	clip[ 2] = aOrientation[ 0] * aProjection[ 2] + aOrientation[ 1] * aProjection[ 6] + aOrientation[ 2] * aProjection[10] + aOrientation[ 3] * aProjection[14];
	clip[ 3] = aOrientation[ 0] * aProjection[ 3] + aOrientation[ 1] * aProjection[ 7] + aOrientation[ 2] * aProjection[11] + aOrientation[ 3] * aProjection[15];

	clip[ 4] = aOrientation[ 4] * aProjection[ 0] + aOrientation[ 5] * aProjection[ 4] + aOrientation[ 6] * aProjection[ 8] + aOrientation[ 7] * aProjection[12];
	clip[ 5] = aOrientation[ 4] * aProjection[ 1] + aOrientation[ 5] * aProjection[ 5] + aOrientation[ 6] * aProjection[ 9] + aOrientation[ 7] * aProjection[13];
	clip[ 6] = aOrientation[ 4] * aProjection[ 2] + aOrientation[ 5] * aProjection[ 6] + aOrientation[ 6] * aProjection[10] + aOrientation[ 7] * aProjection[14];
	clip[ 7] = aOrientation[ 4] * aProjection[ 3] + aOrientation[ 5] * aProjection[ 7] + aOrientation[ 6] * aProjection[11] + aOrientation[ 7] * aProjection[15];

	clip[ 8] = aOrientation[ 8] * aProjection[ 0] + aOrientation[ 9] * aProjection[ 4] + aOrientation[10] * aProjection[ 8] + aOrientation[11] * aProjection[12];
	clip[ 9] = aOrientation[ 8] * aProjection[ 1] + aOrientation[ 9] * aProjection[ 5] + aOrientation[10] * aProjection[ 9] + aOrientation[11] * aProjection[13];
	clip[10] = aOrientation[ 8] * aProjection[ 2] + aOrientation[ 9] * aProjection[ 6] + aOrientation[10] * aProjection[10] + aOrientation[11] * aProjection[14];
	clip[11] = aOrientation[ 8] * aProjection[ 3] + aOrientation[ 9] * aProjection[ 7] + aOrientation[10] * aProjection[11] + aOrientation[11] * aProjection[15];

	clip[12] = aOrientation[12] * aProjection[ 0] + aOrientation[13] * aProjection[ 4] + aOrientation[14] * aProjection[ 8] + aOrientation[15] * aProjection[12];
	clip[13] = aOrientation[12] * aProjection[ 1] + aOrientation[13] * aProjection[ 5] + aOrientation[14] * aProjection[ 9] + aOrientation[15] * aProjection[13];
	clip[14] = aOrientation[12] * aProjection[ 2] + aOrientation[13] * aProjection[ 6] + aOrientation[14] * aProjection[10] + aOrientation[15] * aProjection[14];
	clip[15] = aOrientation[12] * aProjection[ 3] + aOrientation[13] * aProjection[ 7] + aOrientation[14] * aProjection[11] + aOrientation[15] * aProjection[15];




	myCachedNormals[F_RIGHT] = Vector3f(clip[3] - clip[0], clip[7] - clip[4], clip[11] - clip[8]);
	myCachedDistances[F_RIGHT] = clip[15] - clip[12];
	float t = sqrt(myCachedNormals[F_RIGHT].x * myCachedNormals[F_RIGHT].x + myCachedNormals[F_RIGHT].y * myCachedNormals[F_RIGHT].y + myCachedNormals[F_RIGHT].z * myCachedNormals[F_RIGHT].z);
	myCachedNormals[F_RIGHT].x /= t;
	myCachedNormals[F_RIGHT].y /= t;
	myCachedNormals[F_RIGHT].z /= t;
	myCachedDistances[F_RIGHT] /= t;

	myCachedNormals[F_LEFT] = Vector3f(clip[3] + clip[0], clip[7] + clip[4], clip[11] + clip[8]);
	myCachedDistances[F_LEFT] = clip[15] + clip[12];
	t = sqrt(myCachedNormals[F_LEFT].x * myCachedNormals[F_LEFT].x + myCachedNormals[F_LEFT].y * myCachedNormals[F_LEFT].y + myCachedNormals[F_LEFT].z * myCachedNormals[F_LEFT].z);
	myCachedNormals[F_LEFT].x /= t;
	myCachedNormals[F_LEFT].y /= t;
	myCachedNormals[F_LEFT].z /= t;
	myCachedDistances[F_LEFT] /= t;

	myCachedNormals[F_BOTTOM] = Vector3f(clip[3] + clip[1], clip[7] + clip[5], clip[11] + clip[9]);
	myCachedDistances[F_BOTTOM] = clip[15] + clip[13];
	t = sqrt(myCachedNormals[F_BOTTOM].x * myCachedNormals[F_BOTTOM].x + myCachedNormals[F_BOTTOM].y * myCachedNormals[F_BOTTOM].y + myCachedNormals[F_BOTTOM].z * myCachedNormals[F_BOTTOM].z);
	myCachedNormals[F_BOTTOM].x /= t;
	myCachedNormals[F_BOTTOM].y /= t;
	myCachedNormals[F_BOTTOM].z /= t;
	myCachedDistances[F_BOTTOM] /= t;

	myCachedNormals[F_TOP] = Vector3f(clip[3] - clip[1], clip[7] - clip[5], clip[11] - clip[9]);
	myCachedDistances[F_TOP] = clip[15] - clip[13];
	t = sqrt(myCachedNormals[F_TOP].x * myCachedNormals[F_TOP].x + myCachedNormals[F_TOP].y * myCachedNormals[F_TOP].y + myCachedNormals[F_TOP].z * myCachedNormals[F_TOP].z);
	myCachedNormals[F_TOP].x /= t;
	myCachedNormals[F_TOP].y /= t;
	myCachedNormals[F_TOP].z /= t;
	myCachedDistances[F_TOP] /= t;

	myCachedNormals[F_FAR] = Vector3f(clip[3] - clip[2], clip[7] - clip[6], clip[11] - clip[10]);
	myCachedDistances[F_FAR] = clip[15] - clip[14];
	t = sqrt(myCachedNormals[F_FAR].x * myCachedNormals[F_FAR].x + myCachedNormals[F_FAR].y * myCachedNormals[F_FAR].y + myCachedNormals[F_FAR].z * myCachedNormals[F_FAR].z);
	myCachedNormals[F_FAR].x /= t;
	myCachedNormals[F_FAR].y /= t;
	myCachedNormals[F_FAR].z /= t;
	myCachedDistances[F_FAR] /= t;

	myCachedNormals[F_NEAR] = Vector3f(clip[3] + clip[2], clip[7] + clip[6], clip[11] + clip[10]);
	myCachedDistances[F_NEAR] = clip[15] + clip[14];
	t = sqrt(myCachedNormals[F_NEAR].x * myCachedNormals[F_NEAR].x + myCachedNormals[F_NEAR].y * myCachedNormals[F_NEAR].y + myCachedNormals[F_NEAR].z * myCachedNormals[F_NEAR].z);
	myCachedNormals[F_NEAR].x /= t;
	myCachedNormals[F_NEAR].y /= t;
	myCachedNormals[F_NEAR].z /= t;
	myCachedDistances[F_NEAR] /= t;

	SetTransformation(aRealOrientation);
}